#include <Servo.h>

#define pingPin 7
#define STRAIGHT 84

Servo steerServo,pwrServo;
int distance;

void setup() {
steerServo.attach(5);
pwrServo.attach(6);
Serial.begin(4800);
}

void loop() {
  //distance in cm
  if(distance < 250 && distance != 0) {
    //back up
    pwrServo.write(85);
    steerServo.write(STRAIGHT);
    wait(5000);
  }
  
  pwrServo.write(100);
  steerServo.write(STRAIGHT);
  wait(500);
}


int ping() {
 // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:


  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  int duration = pulseIn(pingPin, HIGH);
  
  //convert microseconds to centimeters
  int distance = duration / 29 / 2;
  
  return distance;
}

void wait(long time) {
  for(int i = 0;i<time;i+=100) {
    distance = ping();
    Serial.println(distance);
    delay(100);
  }
}
